#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "sensor_msgs/msg/compressed_image.hpp"

#include <chrono>
#include "opencv2/opencv.hpp"
#include "cv_bridge/cv_bridge.h"


// 自定义节点类
class Camera1_Play : public rclcpp::Node
{
public:
  Camera1_Play() : Node("Camera_1_Node")
  {
    // 创建消息订阅者，从摄像头节点订阅图像
    cam1_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
        "/image_raw/camera_1", 10, std::bind(&Camera1_Play::cam1_callback, this, std::placeholders::_1));
    // 创建消息发布，从该节点发布处理后的图像数据
    cam1_pub_ = this->create_publisher<sensor_msgs::msg::CompressedImage>("camera_1", 1);
  }

private:
  // 从摄像头节点订阅图像
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr cam1_sub_;
  // 发布图像
  rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr cam1_pub_;

  // camera_1的处理
  void cam1_callback(const sensor_msgs::msg::Image::SharedPtr img_msg)
  {

    // 将压缩图像消息解码为cv::Mat
    cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(img_msg, "bgr8");
    cv::Mat image = cv_ptr->image;
    // 对图像进行处理
    std::vector<uchar> compressed_image_buffer;
    cv::imencode(".jpg", image, compressed_image_buffer, {cv::IMWRITE_JPEG_QUALITY, 50}); // 压缩为JPEG格式，质量为90
    // 将压缩后的图像重新编码为CompressedImage消息
    sensor_msgs::msg::CompressedImage compressed_msg;
    compressed_msg.header = img_msg->header;
    compressed_msg.format = "jpeg";
    compressed_msg.data = std::vector<uint8_t>(compressed_image_buffer.begin(), compressed_image_buffer.end());
    // 发布压缩后的图像消息
    cam1_pub_->publish(compressed_msg);
  };
};

int main(int argc, char const *argv[])
{
  // 初始化ROS2客户端
  rclcpp::init(argc, argv);
  // 调用spin函数，并传入节点对象指针
  rclcpp::spin(std::make_shared<Camera1_Play>());
  // 资源释放
  rclcpp::shutdown();
  return 0;
}